/*
 * File: ControlAlgorithm.java
 * 
 * Change Log
 * Author				DTG				Description of Change
 * ---------------------------------------------------------------------------
 * Timothy M. Lemm		18OCT11 0749	Initial Template Structure
 * 										
 */


/*
 * ATTENTION!:
 * This file is used to implement the iterations specific control algorithm.
 * 
 * There are Android Specific routines contained within this template. If you 
 * do not know what you are doing DO NOT modify anything outside of the comment markers
 * that follow the form:
 * //PLACE ALGORITHM BELOW THIS LINE
 * //PLACE ALGORITHM ABOVE THIS LINE
 * 
 * Information on available functions to be used can be found at:
 * code.google.com/p/erau-droidsat-framework/wiki/ControlAlgorithm
 */


package com.erau.droidsatframework.control;

import com.erau.droidsatframework.control.ControlService.ControlBinder;
import android.app.Service;
import android.content.ComponentName;
import android.content.Context;
import android.content.Intent;
import android.content.ServiceConnection;
import android.os.IBinder;

public class ControlAlgorithmService extends Service {
	
	ControlService control;				//Local handle of the ControlService Instance for binding
	
	@Override
	public IBinder onBind(Intent intent) {
		return null; //Nothing binds to the algorithm
	}
	
	@Override
	public void onCreate() {
		//Bind Back to Core...
		Intent ctlAlg2CtlIntent = new Intent(this, ControlService.class); //Intent to bind
		bindService(ctlAlg2CtlIntent, bindConnection, Context.BIND_AUTO_CREATE ); //Bind
		
	}
	
	@Override
	public int onStartCommand(Intent intent, int flags, int startId) {
		return START_NOT_STICKY; //We can die
	}//End onStartCommand
	
	private ServiceConnection bindConnection = new ServiceConnection() {

		@Override
		public void onServiceConnected(ComponentName name, IBinder service) {
			ControlBinder ctlBinder = (ControlBinder) service; //cast the call back service into the core binder
			control = ctlBinder.getService();	//bring in the active instance of the core service
			
			double pos = control.getCompassPositionX();
			control.testMethod();
			// Once we are bound we will allow the AEs to run their "code"
			controlAlogrithm();

		}

		@Override
		public void onServiceDisconnected(ComponentName name) {
			//Currently dont handle this
		}
	};
	
	private void controlAlogrithm() {
	// PLACE ALGORITHM BELOW THIS LINE!	
		double gyro_angle_z, gyro_rate_z;
		double target;
		double z_error;
		int motor_velocity, motor_accel, motor_axis;
		double meh;
		final double PI;
		final double M;
		final double D;
		
		//Make Control Constants Available
		CONTROL_CONSTANTS CONSTANT = new CONTROL_CONSTANTS();
		PI=CONTROL_CONSTANTS.PI;
		M=CONTROL_CONSTANTS.M_Z_AXIS;
		D=CONTROL_CONSTANTS.D_Z_AXIS;
		
		//TEST PURPOSE: Setting target to 90 degrees
		target=CONSTANT.PI/2;
		
		motor_accel=500;
		motor_axis=CONSTANT.Z_AXIS;
		
				
		while (true) {
			//Update Incoming Gyro Angles and Rates in rad and rad/s respectivly
			gyro_angle_z=control.getGyroAngleZ();
			gyro_rate_z=control.getGyroRateZ();

			//Angle error in radians
			z_error=gyro_angle_z-target;
			
			//Phase Plane Variable
			meh=(gyro_rate_z/M)-z_error;
					
			if (meh < D && meh > D) {
				motor_velocity=0;
			} else if (meh < D) {
				motor_velocity = 800;
			} else {
				motor_velocity = -800;
			}
			
			//Output desired motor specifications to control service
			control.commandRequest(motor_velocity, motor_accel, motor_axis);
		}
	// PLACE ALGORITHM ABOVE THIS LINE!	
	}

}
